{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "4f4bfde1",
   "metadata": {},
   "source": [
    "# High-Precision Propagation\n",
    "\n",
    "This example demonstrates high-precision orbit propagation using satkit's advanced numerical integration capabilities. It propagates a GPS satellite orbit over a 24-hour period and validates the results against high-fidelity SP3 ephemeris data from the European Space Agency (ESA).\n",
    "\n",
    "## Overview\n",
    "\n",
    "The example performs the following steps:\n",
    "\n",
    "1. **Loads Reference Data**: Reads GPS satellite positions from an SP3 file containing precise orbit determination results (note: velocity is not included)\n",
    "2. **Rotate to Inertial Frame**: Rotate the GPS satellite positions from the ITRF (Earth-fixed) to the GCRF (inertial) frame\n",
    "3. **Fits Initial Conditions**: Use the high-precision propagator to output the state and state transition matrix at the SP3 timestamps. Use that matrix to linearize the system and solve for corrections to the initial state (position and velocity) that minimize errors over the 1-day data set.\n",
    "The least-squares fit minimizes: $$||p_{gps}(t) - \\hat{p}(t; s_0) - \\Phi_p(t; s_0) \\, \\Delta s_0||^2$$\n",
    "where $p_{gps}(t)$ is the SP3 position in GCRF, $\\hat{p}(t; s_0)$ is the propagated position from initial state $s_0$, $\\Phi_p(t; s_0)$ is the position block of the state transition matrix, and $\\Delta s_0$ is the correction to the initial state, which is what we are solving for.\n",
    "\n",
    "Note: We wrap a bounded, non-linear solve for solar radiation pressure ($CrA/m$) around the least-squares fit to improve accuracy. This adds complexity but highlights the precision achievable with the high-fidelity model.\n",
    "\n",
    "4. **Validates Results**: Compares the propagated positions against the SP3 truth data and visualizes the position errors\n",
    "\n",
    "This example showcases satkit's ability to achieve meter-level accuracy over extended propagation periods when properly configured with high-fidelity force models and optimized initial conditions.\n",
    "\n",
    "The SP3 file contains a full 24 hours of satellite positions, recorded once every 5 minutes"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e97dd803",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Necessary imports\n",
    "import satkit as sk\n",
    "import numpy as np\n",
    "import math as m\n",
    "import numpy.typing as npt\n",
    "from scipy.optimize import minimize_scalar\n",
    "import plotly.graph_objects as go"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f318566b",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Function to read in the SP3 file\n",
    "\n",
    "def read_sp3file(fname, satnum=20):\n",
    "    \"\"\"\n",
    "    Read SP3 file\n",
    "    (file containing \"true\" GPS ephemerides)\n",
    "    and output UTC time and position in ITRF frame\n",
    "    \"\"\"\n",
    "\n",
    "    # Read in the test vectors\n",
    "    with open(fname, \"r\") as fd:\n",
    "        lines = fd.readlines()\n",
    "\n",
    "    def line2date(lines):\n",
    "        for line in lines:\n",
    "            year = int(line[3:7])\n",
    "            month = int(line[8:10])\n",
    "            day = int(line[11:13])\n",
    "            hour = int(line[14:16])\n",
    "            minute = int(line[17:19])\n",
    "            sec = float(line[20:32])\n",
    "            yield sk.time(year, month, day, hour, minute, sec)\n",
    "\n",
    "    def line2pos(lines):\n",
    "        for line in lines:\n",
    "            lvals = line.split()\n",
    "            yield np.array([float(lvals[1]), float(lvals[2]), float(lvals[3])])\n",
    "\n",
    "    datelines = list(filter(lambda x: x[0] == \"*\", lines))\n",
    "    match = f\"PG{satnum:02d}\"\n",
    "    satlines = list(filter(lambda x: x[0:4] == match, lines))\n",
    "    dates = np.fromiter(line2date(datelines), sk.time)\n",
    "    pitrf = np.stack(np.fromiter(line2pos(satlines), list), axis=0) * 1.0e3  # type: ignore\n",
    "\n",
    "    return (pitrf, dates)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "15735e98",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Download SP3 file if not present\n",
    "fname = './ESA0OPSFIN_20233640000_01D_05M_ORB.SP3'\n",
    "url = \"http://navigation-office.esa.int/products/gnss-products/2294/ESA0OPSFIN_20233640000_01D_05M_ORB.SP3.gz\"\n",
    "import os\n",
    "if not os.path.exists(fname):\n",
    "    import urllib.request\n",
    "    import gzip\n",
    "    with urllib.request.urlopen(url) as response:\n",
    "        with open(fname, 'wb') as out_file:\n",
    "            with gzip.GzipFile(fileobj=response) as uncompressed:\n",
    "                out_file.write(uncompressed.read())\n",
    "\n",
    "# Read in the SP3 file\n",
    "[pitrf, timearr] = read_sp3file(fname)\n",
    "\n",
    "\n",
    "# Rotate positions to the GCRF frame\n",
    "pgcrf = np.stack(\n",
    "    np.fromiter(\n",
    "        (q * p for q, p in zip(sk.frametransform.qitrf2gcrf(timearr), pitrf)), list # type: ignore\n",
    "    ),\n",
    "    axis=0,\n",
    ") # type: ignore\n",
    "# Crude estimation of initial velocity is just the difference between the 1st two position states divided by the difference\n",
    "# in time.  This will have a very high error value, since the two points are 5 minutes apart.\n",
    "vgcrf = (pgcrf[1, :] - pgcrf[0, :]) / (timearr[1] - timearr[0]).seconds\n",
    "\n",
    "# Create the initial state\n",
    "state0 = np.concatenate((pgcrf[0, :], vgcrf))\n",
    "\n",
    "# Settings to use in propagations\n",
    "settings = sk.propsettings()\n",
    "# Only compute sun, moon positions and earth rotation vectors once for all propagations\n",
    "settings.precompute_terms(timearr[0], timearr[-1])\n",
    "settings.abs_error = 1e-10\n",
    "settings.rel_error = 1e-12\n",
    "settings.gravity_order = 10\n",
    "\n",
    "def linearized_least_squares_fit(state0, timearr, pgcrf, settings, sp, iters=5):\n",
    "    \"\"\"\n",
    "    Linearized least squares fit of initial state to SP3 truth data, holding CrAoverM fixed.\n",
    "    \"\"\"\n",
    "    state0_s = state0.copy()\n",
    "    for idx in range(iters):\n",
    "        # Propagate state and state transition matrix over times of interest\n",
    "        res = sk.propagate(state0_s, timearr[0], timearr[-1], output_phi=True, propsettings=settings, satproperties=sp)\n",
    "\n",
    "        # Get state and state transition matrix at times of GPS truth data\n",
    "        statearr, phiarr = zip(*[res.interp(t, output_phi=True) for t in timearr])\n",
    "        phiarr = np.array(phiarr)\n",
    "        statearr = np.array(statearr)\n",
    "\n",
    "        # Linearized least squares solve for state0 update\n",
    "        H = np.sum([p[0:3,:].T @ p[0:3,:] for p in phiarr], axis=0)\n",
    "        b = np.sum([p[0:3,:].T @ (pgcrf[i, :] - statearr[i, 0:3]).T for i, p in enumerate(phiarr)], axis=0)\n",
    "        dstate0 = np.linalg.solve(H, b)\n",
    "        state0_s = state0_s + dstate0\n",
    "\n",
    "    perr = np.zeros((len(timearr), 3))\n",
    "    for i in range(len(timearr)):\n",
    "        perr[i, :] = res.interp(timearr[i])[0:3] - pgcrf[i, :]\n",
    "\n",
    "    return state0_s, res, perr\n",
    "\n",
    "# Wrap linearized solve inside a bounded non-linear optimization of CrAoverM\n",
    "def minfunc(v, state0, timearr, pgcrf, settings):\n",
    "    sp = sk.satproperties_static(craoverm = v)\n",
    "    _, _, perr = linearized_least_squares_fit(state0, timearr, pgcrf, settings, sp)\n",
    "    return np.sum(np.sum(perr ** 2, axis=1), axis=0)\n",
    "\n",
    "# Non-linear minimization of CrAoverM, with linearized least squares fit of initial state at each step\n",
    "v0 = 0.01\n",
    "r = minimize_scalar(\n",
    "    lambda v: minfunc(v, state0, timearr, pgcrf, settings),\n",
    "    bounds=(0, 1),\n",
    "    method='bounded',\n",
    "  )\n",
    "\n",
    "# Final least squares fit with optimized CrAoverM\n",
    "sp = sk.satproperties_static(craoverm = r.x)\n",
    "state0, res, perr = linearized_least_squares_fit(state0, timearr, pgcrf, settings, sp)\n",
    "\n",
    "# print the mean error\n",
    "print(f\"Satellite radiation pressure susceptibility, Cr A over M: {r.x:.4f} m^2/kg\")\n",
    "print(f\"Mean position error of fit over 24 hours: {np.mean(np.linalg.norm(perr, axis=1)):.3f} meters\")\n",
    "\n",
    "# Plot position error\n",
    "fig = go.Figure()\n",
    "# Add markers for each component of the position error\n",
    "fig.add_trace(\n",
    "    go.Scatter(x=[t.datetime() for t in timearr], y=perr[:, 0], mode=\"lines\", name=\"X\")\n",
    ")\n",
    "fig.add_trace(\n",
    "    go.Scatter(x=[t.datetime() for t in timearr], y=perr[:, 1], mode=\"lines\", name=\"Y\")\n",
    ")\n",
    "fig.add_trace(\n",
    "    go.Scatter(x=[t.datetime() for t in timearr], y=perr[:, 2], mode=\"lines\", name=\"Z\")\n",
    ")\n",
    "fig.update_layout(\n",
    "    title=\"Propagation Error vs SP3 Truth for GPS Satellite\",\n",
    "    title_font_size=18,\n",
    "    xaxis_title=\"Time\",\n",
    "    yaxis_title=\"Position Error (m)\",\n",
    "    xaxis_title_font=dict(size=16),\n",
    "    yaxis_title_font=dict(size=16),\n",
    "    legend_font=dict(size=14),\n",
    "    font=dict(size=14),\n",
    ")\n",
    "fig.show()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.14.0"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}